#include "AlgoPRM.h"
#include "AlgoNSGA.h" // Include NSGA for the optimization phase
#include "AlgoEvaluator.h" // Required for NSGA interface
#include <iostream>
#include <vector>
#include "DebugConfig.h"

int main()
{
    // 0. Load Debug Configuration
    DebugConfig::getInstance().load("debug_config.ini");

    // 1. Setup Task Configuration
    algobase::TaskConfig taskConfig;
    taskConfig.mMinLongitude = 0;
    taskConfig.mMaxLongitude = 10;
    taskConfig.mMinLatitude = 0;
    taskConfig.mMaxLatitude = 10;
    taskConfig.mStartPos = { 1, 1 }; // Adjusted to be clearly in a valid area
    taskConfig.mEndPos = { 9, 9 };   // Adjusted to be clearly in a valid area

    // Add a simple obstacle to make the problem non-trivial
    taskConfig.mTypePolygons[algobase::AreaType::AT_Restricted].push_back({
        {3, 3}, {7, 3}, {7, 7}, {3, 7}
    });

    // 2. Create Mock Terrain Data (a simple flat sea bottom)
    std::vector<std::vector<double>> terrain_data(100, std::vector<double>(100, -100.0)); // 100x100 grid, depth -100

    // 3. PRM: Generate Initial Population
    algo::AlgoPRM prm_algo;
    algobase::Paths initial_paths;

    DEBUG_LOG("TestRunner", "--- PRM Phase ---");
    DEBUG_LOG("TestRunner", "Building roadmap...");
    int build_code = prm_algo.buildRoadmap(taskConfig, terrain_data);
    if (build_code != 0) {
        DEBUG_LOG("TestRunner", "Failed to build roadmap. Error code: " << build_code);
        return -1;
    }
    DEBUG_LOG("TestRunner", "Roadmap built.");

    DEBUG_LOG("TestRunner", "Finding initial paths...");
    int find_code = prm_algo.findPaths(taskConfig, initial_paths);
    if (find_code != 0) {
        DEBUG_LOG("TestRunner", "Failed to find paths. Error code: " << find_code);
        return -1;
    }
    DEBUG_LOG("TestRunner", "Found " << initial_paths.size() << " initial paths.");

    if (initial_paths.empty()) {
        DEBUG_LOG("TestRunner", "PRM generated 0 paths. The test cannot proceed to NSGA.");
        return 0;
    }

    // 4. NSGA: Optimize the Initial Population
    algo::AlgoNSGA nsga_algo;
    algo::AlgoEvaluator evaluator; // A real evaluator instance
    algobase::NSGAParams nsga_params;
    std::vector<algobase::TrajectoryCoords> optimized_paths;

	algobase::EvaluateL2IndexWeightVector Safe, Stealth, Economy, Completion;
	Safe.emplace_back(algobase::EvaluateL2IndexType::EL2IT_0Topography, 0.2);
	Stealth.emplace_back(algobase::EvaluateL2IndexType::EL2IT_3CPCD, 0.7);
	Economy.emplace_back(algobase::EvaluateL2IndexType::EL2IT_2Turbulence, 0.5);
	Completion.emplace_back(algobase::EvaluateL2IndexType::EL2IT_7NavigationTime, 1.0);

	algobase::EvaluateL1IndexMap L1Indexmap;
	L1Indexmap[algobase::EL1IT_Safety].first = 0.2;
	L1Indexmap[algobase::EL1IT_Safety].second = Safe;
	L1Indexmap[algobase::EL1IT_Stealth].first = 0.3;
	L1Indexmap[algobase::EL1IT_Stealth].second = Stealth;
	L1Indexmap[algobase::EL1IT_Economy].first = 0.3;
	L1Indexmap[algobase::EL1IT_Economy].second = Economy;
	L1Indexmap[algobase::EL1IT_Completion].first = 0.2;
	L1Indexmap[algobase::EL1IT_Completion].second = Completion;

    DEBUG_LOG("TestRunner", "\n--- NSGA Phase ---");
    DEBUG_LOG("TestRunner", "Optimizing initial population of " << initial_paths.size() << " paths...");
    
    evaluator.setTaskConfig(taskConfig);
    evaluator.setEvaluateL1IndexMap(L1Indexmap);
    int nsga_code = nsga_algo.NSGA(&evaluator, nsga_params, initial_paths, optimized_paths);
    
    if (nsga_code != 0) {
        DEBUG_LOG("TestRunner", "NSGA optimization failed. Error code: " << nsga_code);
        return -1;
    }
    DEBUG_LOG("TestRunner", "NSGA finished. Produced " << optimized_paths.size() << " optimized paths.");

    DEBUG_LOG("TestRunner", "\nTest finished successfully.");

    return 0;
}